
#include <string.h>
#include "fifo2.h"

#define min(a, b) ((a) < (b) ? (a) : (b))
#if 1
static inline unsigned int is_power_of_2(unsigned int n)
{
    return (n != 0 && ((n & (n - 1)) == 0));
}

static unsigned int round_down_power_of_two(unsigned int n)
{
    unsigned int round = n;
    unsigned int i;

    if(round == 0 || is_power_of_2(round))
    {
        return round;
    }

    for(i = 0; i < (sizeof(n) * 8); i++)
    {
        round >>= 1;

        if(round == 0)
        {
            break;
        }
    }

    return 1u << i;
}
#endif
/**
 * fifo_init - init a new FIFO using a  buffer
 * @buffer: the preallocated buffer to be used.
 * @size: the size of the internal buffer, this have to be a power of 2.
 *
 */
fifo_t *fifo_init(fifo_t *fifo, elem_t *buffer, unsigned int size)
{
    if (fifo)
    {
        fifo->buffer = buffer;
        fifo->size = round_down_power_of_two(size);
        fifo->in = fifo->out = 0;
    }
	else
	{
		while(1){}
	}
    return fifo;
}


/**
 * fifo_get - gets some data from the FIFO, no locking version
 * @fifo: the fifo to be used.
 * @buffer: where the data must be copied.
 * @len: the size of the destination buffer.
 *
 * This function copies at most @len bytes from the FIFO into the
 * @buffer and returns the number of copied bytes.
 *
 * Note that with only one concurrent reader and one concurrent
 * writer, you don't need extra locking to use these functions.
 */
unsigned int fifo_get_block(fifo_t *fifo, elem_t *buffer, unsigned int len)
{
    unsigned int l;
    unsigned int in = fifo->in;
    unsigned int out = fifo->out;
    unsigned int size = fifo->size;
    unsigned int out_offset = out & (size - 1);
//    unsigned int out_offset = out % size;
    elem_t *buf = fifo->buffer;

    /*
    两个unsigned  变量相减的结果为两个变量的循环间隔。
    如 unsigned char 1 - unsigned char 2 = 0xFF, unsigned char 0 - unsigned char 0xFF = 1.
    */
    len = min(len, in - out);

    /* first get the data from fifo->out until the end of the buffer */
    l = min(len, size - out_offset);
    memcpy(buffer, buf + out_offset, l * sizeof(elem_t));

    /* then get the rest (if any) from the beginning of the buffer */
    memcpy(buffer + l, buf, (len - l) * sizeof(elem_t));
	out += len;	
    fifo->out = out;
    return len;
}


/**
 * kfifo_put - puts some data into the FIFO, no locking version
 * @fifo: the fifo to be used.
 * @buffer: the data to be added.
 * @len: the length of the data to be added.
 *
 * This function copies at most @len bytes from the @buffer into
 * the FIFO depending on the free space, and returns the number of
 * bytes copied.
 *
 * Note that with only one concurrent reader and one concurrent
 * writer, you don't need extra locking to use these functions.
 */
unsigned int fifo_put_block(fifo_t *fifo, const elem_t *buffer, unsigned int len)
{
    unsigned int l;
    unsigned int in = fifo->in;
    unsigned int out = fifo->out;
    unsigned int size = fifo->size;
    unsigned int in_offset = in & (size - 1);
//    unsigned int in_offset = in % size;
    elem_t *buf = fifo->buffer;

    /*
    	free_space = size - data_space

    	data_space = in - out

    	free_space = size - (in - out) = size - in + out
    */
    len = min(len, size - in + out);

    /* first put the data starting from fifo->in to buffer end */
    l = min(len, size - in_offset);
    memcpy(buf + in_offset, buffer, l * sizeof(elem_t));

    /* then put the rest (if any) at the beginning of the buffer */
    memcpy(buf, buffer + l, (len - l) * sizeof(elem_t));

	in += len;
    fifo->in = in;

    return len;
}

/*Put one element to fifo*/
unsigned int fifo_put(fifo_t *fifo, elem_t *elem)
{
    unsigned int in = fifo->in;
    unsigned int out = fifo->out;
	const unsigned int size = fifo->size;

    /*have free space ?*/
    if((size - in + out) > 0u)
    {    	
        unsigned int in_offset = in & (size - 1);
        elem_t *buf = fifo->buffer;

        buf[in_offset] = *elem;
        in++;
        fifo->in = in;
        return 1;
    }

    return 0;
}

/*Get one element from fifo*/
unsigned int fifo_get(fifo_t *fifo, elem_t *elem)
{
    unsigned int in = fifo->in;
    unsigned int out = fifo->out;

    /*have data?*/
    if(in != out)
    {    	
		const unsigned int size = fifo->size;
        unsigned int out_offset = out &(size - 1);
        elem_t *buf = fifo->buffer;

        *elem = buf[out_offset];
        out++;
        fifo->out = out;
        return 1;
    }

    return 0;
}

